function xdot = der(t,x,param)
t
zeta11 = param{1};
zeta12 = param{2};
zeta21 = param{3};
zeta22 = param{4};
Zeta14 = param{5};
Zeta24 = param{6};
inertia = param{7};
mass = param{8};
GRAVITY = param{9};
cmtoH1 = param{10};
cmtoH2= param{11};
shift_inverse_H1 = param{12};
shift_inverse_H2 = param{13};
NBODIES = param{14};

q = x(1:NBODIES,1);
u = x(NBODIES+1:end,1);

% Preallocating variable sizes
Wd = zeros(6,NBODIES,NBODIES);
pr_c_k = zeros(3,3,NBODIES);
Pr_C_K = zeros(6,6,NBODIES);
n_c_k = zeros(3,3,NBODIES);

Zeta14_asm = zeros(6,NBODIES,NBODIES);
Zeta24_asm = zeros(6,NBODIES,NBODIES);
zeta13 = zeros(6,NBODIES);
zeta23 = zeros(6,NBODIES);

% For a revolute joint about positive z axis
% H is referred to as P^J_k in our paper. 
H = [0 0 1 0 0 0]';

% D = D^J_k
D = [1 0 0 0 0;
     0 1 0 0 0;
     0 0 0 0 0;
     0 0 1 0 0;
     0 0 0 1 0;
     0 0 0 0 1];
 
Dk = [0 0 ;
      0 0;
      0 0 ;
      1 0 ;
      0 1 ;
      0 0 ];


for i=1:NBODIES
    % Transformation matrix between body and parent
    pr_c_k(:,:,i) = [cos(q(i)) -sin(q(i)) 0;
                     sin(q(i)) cos(q(i)) 0;
                     0         0         1];
    
    % Form spatial trasnformation matrices
    Pr_C_K(:,:,i) = [pr_c_k(:,:,i) zeros(3,3);
        zeros(3,3) pr_c_k(:,:,i)];
    
    % Transformation matrix between body and newtonian frame
    n_c_k(:,:,i) = [cos(sum(q(1:i))) -sin(sum(q(1:i))) 0;
        sin(sum(q(1:i))) cos(sum(q(1:i))) 0;
        0                0                1];
%     N_C_K(:,:,i) = [n_c_k(:,:,i) zeros(3,3);
%         zeros(3,3)   n_c_k(:,:,i)];
    % Angular velocity of each body is trivially obtained here
    W(:,i) = [0 0 sum(u(1:i))]';
    
    Fa  = [ -cross(W(:,i),inertia(:,:,i)*W(:,i)); % -w x Iw term
        mass(i)*n_c_k(:,:,i)'*GRAVITY];% Only external force is gravity
    
    At_H1  = [zeros(3,1);cross(W(:,i),cross(W(:,i),cmtoH1(:,i)))];
    At_H2  = [zeros(3,1);cross(W(:,i),cross(W(:,i),cmtoH2(:,i)))];
    
    % phi13 and phi23 are also written in body basis
    zeta13(:,i) = shift_inverse_H1(:,:,i)*Fa + At_H1; % (1a)
    zeta23(:,i) = shift_inverse_H2(:,:,i)*Fa + At_H2; % (1b)
end


% A convenient tmp variable.
zeta11_asm_tmp = zeta11(:,:,NBODIES);
zeta12_asm_tmp = zeta12(:,:,NBODIES);
zeta13_asm_tmp = zeta13(:,NBODIES);
zeta14_asm_tmp = Zeta14(:,:,NBODIES);

zeta21_asm_tmp = zeta21(:,:,NBODIES);
zeta22_asm_tmp = zeta22(:,:,NBODIES);
zeta23_asm_tmp = zeta23(:,NBODIES);
zeta24_asm_tmp = Zeta24(:,:,NBODIES);

% Assembly
% NBODIES are connected by (NBODIES-1) joint excluding the one which connects
% the first body to ground (if at all)
for i=NBODIES:-1:2
    
    % Convert handle equations of body i to basis of (i-1)
    % Handle 1
    zeta11_c = Pr_C_K(:,:,i)*zeta11_asm_tmp*Pr_C_K(:,:,i)';
    zeta12_c = Pr_C_K(:,:,i)*zeta12_asm_tmp*Pr_C_K(:,:,i)';
    zeta13_c = Pr_C_K(:,:,i)*zeta13_asm_tmp;
    zeta14_c = Pr_C_K(:,:,i)*zeta14_asm_tmp;
    
    % Handle 2
    zeta21_c = Pr_C_K(:,:,i)*zeta21_asm_tmp*Pr_C_K(:,:,i)';
    zeta22_c = Pr_C_K(:,:,i)*zeta22_asm_tmp*Pr_C_K(:,:,i)';
    zeta23_c = Pr_C_K(:,:,i)*zeta23_asm_tmp;
    zeta24_c = Pr_C_K(:,:,i)*zeta24_asm_tmp;
    
    % Assemble bodies i and (i-1)
    X_tilde = inv(D'*(zeta11_c + zeta22(:,:,i-1))*D); % (3e)
    X = D*X_tilde*D'; % (3d)
    
    Wa(:,:,i) = X*zeta21(:,:,i-1);        % (3b)
    Wb(:,:,i) = -X*zeta12_c;               % (3b)
    Wc(:,i) = X*(zeta23(:,i-1)-zeta13_c); % (3c)
    
    Wd(:,:,i) = X*(Zeta24(:,:,i-1)-zeta14_c);
    
    % Note in equation (3c), \dot{P}^J_K u is identically equal to zero for
    % all planar revolute joints and not included in calculations.
    % PS: The sign of equation Wb in (3b) in the paper draft i sent you
    % before was wrong.
    
    % Below are equations (4a)-(4f)
    zeta11_asm(:,:,i) = zeta11(:,:,i-1) - zeta12(:,:,i-1)*Wa(:,:,i);
    zeta12_asm(:,:,i) = -zeta12(:,:,i-1)*Wb(:,:,i);
    zeta13_asm(:,i) = zeta13(:,i-1) - zeta12(:,:,i-1)*Wc(:,i) ;
    Zeta14_asm(:,:,i) = -zeta12(:,:,i-1)*Wd(:,:,i) + Zeta14(:,:,i-1);
    
    zeta21_asm(:,:,i) = zeta21_c*Wa(:,:,i);
    zeta22_asm(:,:,i) = zeta22_c + zeta21_c*Wb(:,:,i);
    zeta23_asm(:,i) = zeta23_c + zeta21_c*Wc(:,i);
    Zeta24_asm(:,:,i) = zeta21_c*Wd(:,:,i) + zeta24_c;
    
    zeta11_asm_tmp = zeta11_asm(:,:,i);
    zeta12_asm_tmp = zeta12_asm(:,:,i);
    zeta13_asm_tmp = zeta13_asm(:,i);
    zeta14_asm_tmp = Zeta14_asm(:,:,i);
    
    zeta21_asm_tmp = zeta21_asm(:,:,i);
    zeta22_asm_tmp = zeta22_asm(:,:,i);
    zeta23_asm_tmp = zeta23_asm(:,i);
    zeta24_asm_tmp = Zeta24_asm(:,:,i);
    
end

XX = -D*inv(D'*zeta11_asm(:,:,2)*D)*D';
psi1 = XX*zeta13_asm(:,2);
psi2 = XX*Zeta14_asm(:,:,2);
psi3 = zeta21_asm(:,:,2)*psi1 + zeta23_asm(:,2);
psi4 = zeta21_asm(:,:,2)*psi2 + Zeta24_asm(:,:,2);
psi5 = Dk'*psi3;
psi6 = Dk'*psi4;

T = -psi6'*inv(psi6*psi6')*psi5;
Tn = null(psi6);
% T = T + 100*Tn(:,end);

% Fh1 = psi1 + psi2*T;
% Ah1 = zeta11_asm(:,:,2)*Fh1 + zeta13_asm(:,2) + zeta14_asm{2}*T;
% Ah2 = zeta21_asm(:,:,2)*Fh1 + zeta23_asm(:,2) + zeta24_asm{2}*T;

% Spatial acceleration of ground/parent
Ah2_parent = zeros(6,1);

% Disassembly
for i=1:NBODIES
    
    Ah2_parent_inchildbasis = Pr_C_K(:,:,i)'*Ah2_parent;
    
    if(i~=NBODIES)
        % For all intermediate assembled systems, the spatial constraint
        % force on handle 2 is zero. Hence the equation of handle 1 of an
        % assembled intermediate system can be written as
        % Ah1 = zeta11_asm*Fh1 + zeta13
        % Since D'*(Ah1 - Ah2_parent) = 0 the above equation reduces to
        % D'*(zeta11_asm*Fh1 + zeta13-Ah2_parent)=0 From this, Fh1 can be
        % calculated as
        Fh1 = D*((D'*zeta11_asm(:,:,i+1)*D) \ (-D'*(zeta13_asm(:,i+1)+Zeta14_asm(:,:,i+1)*T-Ah2_parent_inchildbasis)));
        
        % the stored values of Wa,Wb and Wc are used to calculate the
        % constrained force on hte connecting joint.
        Fh2 = -Wa(:,:,i+1)*Fh1 -Wc(:,i+1) - Wd(:,:,i+1)*T; % This is the negative of eq (3a)
        Ah1 = zeta11_asm(:,:,i+1)*Fh1 + zeta13_asm(:,i+1) + Zeta14_asm(:,:,i+1)*T;
        
        Ah2 = zeta21(:,:,i)*Fh1 + zeta22(:,:,i)*Fh2 + zeta23(:,i) + Zeta24(:,:,i)*T;
        Ah2_parent = Ah2;
    else
        % For the last body, there is no assembly.
        Fh1 = D*((D'*zeta11(:,:,i)*D) \ (-D'*(zeta13(:,i)+Zeta14(:,:,i)*T-Ah2_parent_inchildbasis)));
        Ah1 = zeta11(:,:,i)*Fh1 + zeta13(:,i) + Zeta14(:,:,i)*T;
        Ah2 = zeta21(:,:,i)*Fh1 + zeta23(:,i) + Zeta24(:,:,i)*T;
    end
        
    % w_parent x w_child terms is identically equal to zero and not taken
    % into account here
    udot(i,1) = H'*(Ah1 - Ah2_parent_inchildbasis);
end
xdot = [u;udot];